// $Id: telescope.c 477 2014-04-30 22:03:41Z alan.watson.f $

#include "oaxaca-config.h"
#include "oaxaca.h"

#include <math.h>
#include <stdio.h>

static double interval = 5.0;

static unsigned int cycle = 0;

static void
telescope_focus_next(int index)
{
  volatile oaxaca_detector_t *detector = &oaxaca_shm->detector[index];

  if (detector->g[2] > 0 && fabs(oaxaca_shm->wfc.amean[2]) > 0.3) {
    int foffset = oaxaca_shm->wfc.amean[2] > 0 ? +1 : -1;
    FILE *fp = fopen("/tmp/telescope", "w");
    if (fp != NULL) {
      fprintf(fp, "CF%+d;\n", foffset);
      fclose(fp);
    }
    oaxaca_info("telescope: moving focus %+d.", foffset);
  }
}

static void
telescope_offset_next(int index)
{
  volatile oaxaca_detector_t *detector = &oaxaca_shm->detector[index];

  if (
    detector->g[0] > 0 && 
    detector->g[1] > 0 && 
    (fabs(oaxaca_shm->wfc.amean[0]) > 0.3 || fabs(oaxaca_shm->wfc.amean[1]) > 0.3)
  ) {
    int eoffset;
    int noffset;
    if (oaxaca_shm->wfc.amean[0] > +0.3)
      eoffset = +1;
    else if (oaxaca_shm->wfc.amean[0] < -0.3)
      eoffset = -1;
    else
      eoffset = 0;
    if (oaxaca_shm->wfc.amean[1] > +0.3)
      noffset = -1;
    else if (oaxaca_shm->wfc.amean[1] < -0.3)
      noffset = +1;
    else
      noffset = 0;
    oaxaca_info("telescope: offsetting %+d arcsec east and %+d arcsec north.", eoffset, noffset);
    FILE *fp = fopen("/tmp/telescope", "w");
    if (fp != NULL) {
      fprintf(fp, "MF%+04d%+04d;\n", -eoffset, noffset);
      fclose(fp);
    }
  }
}


void
oaxaca_telescope_next(int index)
{
  volatile oaxaca_detector_t *detector = &oaxaca_shm->detector[index];

  if (detector->frame > 0 && detector->frame % (long) (interval * detector->framefrequency) == 0) {
    ++cycle;
    if (cycle % 2 == 0)
      telescope_offset_next(index);
    else
      telescope_focus_next(index);
  }

}
